{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.8.5-final"
    },
    "colab": {
      "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true
    }
  },
  "cells": [
    {
      "cell_type": "code",
      "metadata": {
        "id": "eeMrMI0-1Dhu",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
        "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
        "proc, zmq_url, web_url = start_zmq_server_as_subprocess()\n",
        "\n",
        "# Let's do all of our imports here, too.\n",
        "import numpy as np\n",
        "\n",
        "from meshcat import Visualizer\n",
        "import meshcat.geometry as g\n",
        "import meshcat.transformations as tf"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "QuFmomDOxoAR",
        "colab_type": "code",
        "colab": {},
        "tags": []
      },
      "source": [
        "meshcat = Visualizer(zmq_url=zmq_url)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "meshcat['/Background'].set_property('visible',False)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "def plot_surface(meshcat_viz, X, Y, Z, color=0xdd9999):\n",
        "    (rows, cols) = Z.shape\n",
        "\n",
        "    vertices = np.empty((rows*cols,3),dtype=np.float32)\n",
        "    vertices[:,0] = X.reshape((-1))\n",
        "    vertices[:,1] = Y.reshape((-1))\n",
        "    vertices[:,2] = Z.reshape((-1))\n",
        "\n",
        "    # Vectorized faces code from https://stackoverflow.com/questions/44934631/making-grid-triangular-mesh-quickly-with-numpy\n",
        "    faces = np.empty((cols-1,rows-1,2,3),dtype=np.uint32)\n",
        "    r = np.arange(rows*cols).reshape(cols,rows)\n",
        "    faces[:,:, 0,0] = r[:-1,:-1]\n",
        "    faces[:,:, 1,0] = r[:-1,1:]\n",
        "    faces[:,:, 0,1] = r[:-1,1:]\n",
        "    faces[:,:, 1,1] = r[1:,1:]\n",
        "    faces[:,:, :,2] = r[1:,:-1,None]\n",
        "    faces.shape =(-1,3)\n",
        "    meshcat_viz.set_object(\n",
        "        g.TriangularMeshGeometry(vertices, faces), \n",
        "        g.MeshLambertMaterial(color=color, wireframe=False))\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "tags": []
      },
      "outputs": [],
      "source": [
        "# Make data (from mplot3d surface3d_demo)\n",
        "X = np.arange(-5, 5, 0.25)\n",
        "Y = np.arange(-5, 5, 0.25)\n",
        "X, Y = np.meshgrid(X, Y)\n",
        "R = np.sqrt(X**2 + Y**2)\n",
        "Z = np.sin(R)\n",
        "\n",
        "plot_surface(meshcat[\"surface\"], X, Y, Z)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": []
    }
  ]
}